import numpy as np
from itertools import combinations
from math import sqrt, atan2, acos, asin
from pyproj import Transformer
from shapely.geometry import Point, Polygon, LineString

trans2utm = Transformer.from_crs("EPSG:4326", "EPSG:3857")
trans2lonlat = Transformer.from_crs("EPSG:3857", "EPSG:4326")
eps = 1e5


def path_length(path):
    total_length = 0.0
    for i in range(len(path) - 1):
        total_length += l2norm(path[i], path[i + 1])
    return total_length


def smooth(path, obstacles):
    length = 0.0
    path_smooth = []
    route = path[:]
    path_smooth.append(path[0])
    while True:
        n = len(route)
        if n == 1:
            break
        for i in range(n - 1, 0, -1):
            if i == 1:
                path_smooth.append((route[i][0], route[i][1]))
                length += l2norm(path_smooth[-1], path_smooth[-2])
                for j in range(i):
                    route.pop(0)
                break
            else:
                pos = (route[0][0], route[0][1])
                pos_next = (route[i][0], route[i][1])
                if not is_intersect_polys(pos, pos_next, obstacles):
                    path_smooth.append((route[i][0], route[i][1]))
                    length += l2norm(path_smooth[-1], path_smooth[-2])
                    for j in range(i):
                        route.pop(0)
                    break
    return path_smooth, length


def perp(vector):
    return np.array([vector[1], -vector[0]])


def is_intersect_polys(pos, pos_next, obstacles):
    for obstacle in obstacles:
        if intersect_ploy_edges(pos, pos_next, obstacle.vertices_):
            return True
    return False


def intersect_ploy_edges(pos1, pos2, vertices):
    num_vertices = len(vertices)
    is_intersect = False

    for i in range(num_vertices):
        point = vertices[i].point_
        point_next = vertices[(i + 1) % num_vertices].point_
        is_intersect = seg_is_intersec(pos1, pos2, point, point_next)
        if is_intersect:
            return is_intersect

    return is_intersect


def pos_in_polygons(pos, poly_gons):
    for ob in poly_gons:
        polygon = Polygon(ob.vertices_pos)
        point = Point((pos[0], pos[1]))
        if point.within(polygon) or point.touches(polygon):
            return True
    return False


def circle_polygon_intersect(center, radius, poly_coords):
    """
    Determines whether a circle and a polygon intersect or touch each other.

    Parameters:
    circle_center: coordinates of the center of the circle in the format (x, y)
    circle_radius: radius of the circle
    polygon_coords: coordinates of the vertices of the polygon in the format [(x1, y1), (x2, y2), ...]

    Returns:
    If the circle and the polygon intersect or touch each other, returns True; otherwise, returns False.
    """
    circle = Point(center).buffer(radius)
    polygon = Polygon(poly_coords)
    return circle.intersects(polygon)


def circle_line_intersect(center, radius, p1, p2):
    """
    判断圆与线段是否相交或接触

    参数:
    center: 圆心坐标 (x, y)
    radius: 圆的半径
    line_coords: 线段端点坐标 ps, p2

    返回值:
    如果圆和线段相交或接触，返回True；否则，返回False。
    """
    # 计算点到线段的距离
    distance = point_to_segment_distance(p1, p2, center)

    # 如果距离小于等于圆的半径，说明相交或接触
    return distance < radius


def min_enclosing_circle(points):
    """多边形的最小包围圆"""
    # 从点集中选取一个、两个或三个点
    for k in range(1, 4):
        for pts in combinations(points, k):
            pts = np.array(pts)
            if k == 1:
                center, radius = pts[0], 0.0
            elif k == 2:
                center = (pts[0] + pts[1]) / 2
                radius = np.linalg.norm(pts[0] - center)
            else:
                A, B, C = pts
                a = np.linalg.norm(B - C)
                b = np.linalg.norm(A - C)
                c = np.linalg.norm(A - B)
                s = (a + b + c) / 2
                area_sq = s * (s - a) * (s - b) * (s - c)
                if area_sq > 0:
                    area = sqrt(s * (s - a) * (s - b) * (s - c))
                    radius = a * b * c / (4 * area)
                else:
                    return points[0], 0.
                # Circumcenter from barycentric coordinates
                deter = (A[0] - C[0]) * (B[1] - C[1]) - (B[0] - C[0]) * (A[1] - C[1])
                if deter == 0:
                    continue  # Collinear points; circumcenter doesn't exist
                center_x = ((A[0] ** 2 + A[1] ** 2) * (B[1] - C[1]) + (B[0] ** 2 + B[1] ** 2) * (C[1] - A[1]) + (
                        C[0] ** 2 + C[1] ** 2) * (A[1] - B[1])) / (2 * deter)
                center_y = ((A[0] ** 2 + A[1] ** 2) * (C[0] - B[0]) + (B[0] ** 2 + B[1] ** 2) * (A[0] - C[0]) + (
                        C[0] ** 2 + C[1] ** 2) * (B[0] - A[0])) / (2 * deter)
                center = np.array([center_x, center_y])

            if all(sqrt((x - center[0]) ** 2 + (y - center[1]) ** 2) <= radius for x, y in points):  # 检测一个圆是否包含所有点
                return center, radius

    # 如果以上都没有返回，则取所有点的中心和最远点距离作为最后的尝试
    center = np.mean(points, axis=0)
    radius = np.max(np.linalg.norm(points - center, axis=1))
    return center, radius


def to_left(p, q, s):
    """
    判断点s是否在有向线段pq的左边
    :param p: (x1, y1)
    :param q: (x2, y2)
    :param s: (x, y)
    :return: True or False
    """
    return area2(p, q, s) > 0


def area2(p, q, s):
    """
    正向（逆时针方向）三角形的有向面积
    :param p: (x1, y1)
    :param q: (x2, y2)
    :param s: (x, y)
    :return: 有向面积
    """
    return p[0] * q[1] - p[1] * q[0] + q[0] * s[1] - q[1] * s[0] + s[0] * p[1] - s[1] * p[0]


# 经纬度转utm坐标系，单位为m，x轴指向东，y轴指向北
def lonlat2grid(lonlat_pos):
    x, y = trans2utm.transform(lonlat_pos[1], lonlat_pos[0])
    return [round(x, 3), round(y, 3)]


def grid2lonlat(utm_pos):
    lat, lon = trans2lonlat.transform(utm_pos[0], utm_pos[1])
    return [round(lon, 8), round(lat, 8)]


# 矩形区域的四条边界线
def get_boundaries(area):
    min_x = 999999999999.
    max_x = -999999999999.
    min_y = 999999999999.
    max_y = -999999999999.
    for k in range(len(area) - 1):
        min_x = min(min_x, area[k][0])
        min_y = min(min_y, area[k][1])
        max_x = max(max_x, area[k][0])
        max_y = max(max_y, area[k][1])
    return min_x, max_x, min_y, max_y


def takeSecond(elem):
    return elem[1]


def pedal(p1, p2, p3):
    """
    过p3作p1和p2相连直线的垂线, 计算垂足的坐标
    直线1: 垂足坐标和p3连线
    直线2: p1和p2连线
    两条直线垂直, 且交点为垂足
    :param p1: (x1, y1)
    :param p2: (x2, y2)
    :param p3: (x3, y3)
    :return: 垂足坐标 (x, y)
    """
    if p2[0] != p1[0]:
        # ########## 根据点x1和x2计算线性方程的k, b
        k, b = np.linalg.solve([[p1[0], 1], [p2[0], 1]], [p1[1], p2[1]])  # 得到k和b
        # #######原理: 垂直向量数量积为0
        x = np.divide(((p2[0] - p1[0]) * p3[0] + (p2[1] - p1[1]) * p3[1] - b * (p2[1] - p1[1])),
                      (p2[0] - p1[0] + k * (p2[1] - p1[1])))
        y = k * x + b

    else:  # 点p1和p2的连线垂直于x轴时
        x = p1[0]
        y = p3[1]

    return np.array([x, y])


# keep angle between [-pi, pi]
def wrap(angle):
    while angle >= np.pi:
        angle -= 2 * np.pi
    while angle < -np.pi:
        angle += 2 * np.pi
    return angle


def left_of(a, b, c):
    """
    a: segment endpoint 1
    b: segment endpoint 2
    c: point
    """
    return det(a - c, b - a)  #


def l2norm(p1, p2):
    """ Compute Euclidean distance in 2D domains"""
    p1, p2 = np.array(p1[:2]), np.array(p2[:2])
    return round(np.linalg.norm(p2 - p1), 5)


def l2normsq(x, y):
    return (x[0] - y[0]) ** 2 + (x[1] - y[1]) ** 2


def sqr(a):
    return a ** 2


def unit_normal_vector(p1p2):
    """ Compute the unit normal vector of  vector p1p2"""
    nRight = normalize(np.array([p1p2[1], -p1p2[0]]))
    nLeft = -nRight
    return nLeft, nRight


def linear_equation(p1, p2):
    a = p2[1] - p1[1]
    b = p1[0] - p2[0]
    c = p1[0] * (p1[1] - p2[1]) + p1[1] * (p2[0] - p1[0])
    return a, b, c


def norm(vec):
    return round(np.linalg.norm(vec), 5)


def normalize(vec):
    if np.linalg.norm(vec) > 0:
        return vec / np.linalg.norm(vec)
    else:
        return np.array([0.0, 0.0])


def absSq(vec):
    return np.dot(vec, vec)


def det(p, q):
    return p[0] * q[1] - p[1] * q[0]


def pi_2_pi(angle):  # to -pi-pi
    return round((angle + np.pi) % (2 * np.pi) - np.pi, 5)


def mod2pi(theta):  # to 0-2*pi
    return round(theta - 2.0 * np.pi * np.floor(theta / 2.0 / np.pi), 5)


def is_parallel(vec1, vec2):
    """ 判断二个向量是否平行 """
    assert vec1.shape == vec2.shape, r'输入的参数 shape 必须相同'
    norm_vec1 = np.linalg.norm(vec1)
    norm_vec2 = np.linalg.norm(vec2)
    vec1_normalized = vec1 / norm_vec1
    vec2_normalized = vec2 / norm_vec2
    if norm_vec1 <= 1e-3 or norm_vec2 <= 1e-3:
        return True
    elif 1.0 - abs(np.dot(vec1_normalized, vec2_normalized)) < 1e-3:
        return True
    else:
        return False


def angle_2_vectors(v1, v2):
    v1v2_norm = (np.linalg.norm(v1) * np.linalg.norm(v2))
    if v1v2_norm == 0.0:
        v1v2_norm = 1e-5
    cosdv = np.dot(v1, v2) / v1v2_norm
    if cosdv > 1.0:
        cosdv = 1.0
    elif cosdv < -1.0:
        cosdv = -1.0
    else:
        cosdv = cosdv
    angle = acos(cosdv)
    return angle


# 判断点是否在圆内
def point_in_circle(pos, obj_pos, combinedRadius):
    if l2norm(pos, obj_pos) < combinedRadius:
        return True
    return False


# 判断线段和圆是否相交
def seg_cross_circle(p_1, p_2, obj_pos, combinedRadius):
    p1_in_circle = point_in_circle(p_1, obj_pos, combinedRadius)
    p2_in_circle = point_in_circle(p_2, obj_pos, combinedRadius)
    if p1_in_circle and p2_in_circle:
        return False
    elif (p1_in_circle and not p2_in_circle) or (not p1_in_circle and p2_in_circle):
        return True
    if p_1[0] == p_2[0]:  # 当x相等
        a, b, c = 1, 0, -p_1[0]
    elif p_1[1] == p_2[1]:  # 当y相等
        a, b, c = 0, 1, -p_1[1]
    else:
        a = p_1[1] - p_2[1]
        b = p_2[0] - p_1[0]
        c = p_1[0] * p_2[1] - p_1[1] * p_2[0]
    dist_1 = (a * obj_pos[0] + b * obj_pos[1] + c) ** 2
    dist_2 = (a * a + b * b) * combinedRadius * combinedRadius
    if dist_1 > dist_2:  # 点到直线距离大于半径r  不相交
        return False
    angle_1 = (obj_pos[0] - p_1[0]) * (p_2[0] - p_1[0]) + (obj_pos[1] - p_1[1]) * (p_2[1] - p_1[1])
    angle_2 = (obj_pos[0] - p_2[0]) * (p_1[0] - p_2[0]) + (obj_pos[1] - p_2[1]) * (p_1[1] - p_2[1])
    if angle_1 > 0 and angle_2 > 0:
        return True
    return False


def cross(p1, p2, p3):  # 跨立实验
    x1 = p2[0] - p1[0]
    y1 = p2[1] - p1[1]
    x2 = p3[0] - p1[0]
    y2 = p3[1] - p1[1]
    return x1 * y2 - x2 * y1


def seg_is_intersec(p1, p2, p3, p4):  # 判断线段p1p2与线段p3p4是否相交
    # 快速排斥，以l1、l2为对角线的矩形必相交，否则两线段不相交
    if (max(p1[0], p2[0]) >= min(p3[0], p4[0])  # 矩形1最右端大于矩形2最左端
            and max(p3[0], p4[0]) >= min(p1[0], p2[0])  # 矩形2最右端大于矩形最左端
            and max(p1[1], p2[1]) >= min(p3[1], p4[1])  # 矩形1最高端大于矩形最低端
            and max(p3[1], p4[1]) >= min(p1[1], p2[1])):  # 矩形2最高端大于矩形最低端

        # 若通过快速排斥则进行跨立实验
        if cross(p1, p2, p3) * cross(p1, p2, p4) <= 0 and cross(p3, p4, p1) * cross(p3, p4, p2) <= 0:
            D = True
        else:
            D = False
    else:
        D = False
    return D


def point_line_dist(line_p1, line_p2, p):
    if np.linalg.norm(line_p2 - line_p1) < 1e-5:
        return l2norm(p, line_p2)
    return np.linalg.norm(np.cross(line_p2 - line_p1, line_p1 - p)) / np.linalg.norm(line_p2 - line_p1)


def signed_distance_to_seg(p1, p2, p):
    """
    Calculate the signed distance from point p to the directed line segment (p1, p2).

    Argus:
    p: Coordinates of point p
    p1, p2: Coordinates of the endpoints of the directed line segment

    Returns:
    float
        If the result is greater than 0, the point p is on the left side of the directed line segment,
        otherwise, it's on the right side.
    """
    v1 = np.array(p) - np.array(p1)
    v2 = np.array(p2) - np.array(p1)
    cross_product = np.cross(v2, v1)
    return cross_product


def signed_distance2line(p1, p2, p):
    """
    Calculate the signed distance from point p to the directed line segment (p1, p2).

    Args:
    p: Coordinates of point p (tuple)
    p1: Coordinates of the start point of the directed line segment (tuple)
    p2: Coordinates of the end point of the directed line segment (tuple)

    Returns:
    float
        If the result is greater than 0, the point p is on the left side of the directed line segment,
        otherwise, it's on the right side.
    """
    # Calculate the vectors
    p1 = np.array(p1)
    v1 = np.array(p) - p1
    v2 = np.array(p2) - p1

    # Calculate the signed distance
    signed_distance = np.cross(v2, v1) / np.linalg.norm(v2)

    return signed_distance


def point_to_segment_distance(vector1, vector2, point):
    """
    Computes the squared distance from a line segment with the specified endpoints to a specified point.

    Args:
        vector1 (Vector2): The first endpoint of the line segment.
        vector2 (Vector2): The second endpoint of the line segment.
        point (Vector2): The point to which the squared distance is to be calculated.

    Returns:
        float: The squared distance from the line segment to the point.
    """
    # Compute squared length of the line segment
    line_length_sq = np.dot(vector2 - vector1, vector2 - vector1)

    # If line_length_sq is zero, the segment is a point
    if line_length_sq == 0:
        return np.dot(point - vector1, point - vector1)

    # Compute the projection parameter of the point onto the line
    t = max(0, min(1, np.dot(point - vector1, vector2 - vector1) / line_length_sq))

    # Compute the closest point on the line segment
    closest_point = vector1 + t * (vector2 - vector1)

    # Return the squared distance between the point and the closest point on the line segment
    return np.dot(point - closest_point, point - closest_point)


def determin_between_line(vector1, vector2, point):
    """
        Determine between line segments.
        Args:
            vector1 (Vector2): The first endpoint of the line segment.
            vector2 (Vector2): The second endpoint of the line segment.
            point (Vector2): The point to which the squared distance is to be calculated.
        Returns:
            bool
        """
    vec = vector2 - vector1
    sq_vec = np.dot(vec, vec)
    if sq_vec == 0:
        return False
    r = np.dot((point - vector1), vec) / sq_vec

    return True if 0.0 <= r <= 1.0 else False


def scal(data, sec_dis):
    """多边形等距缩放

    Args:
        data: 多边形按照逆时针顺序排列的的点集
        sec_dis: 缩放距离

    Returns:
        缩放后的多边形点集
    """
    num = len(data)
    scal_data = []
    for k in range(num):
        x1 = data[k % num][0] - data[(k - 1) % num][0]
        y1 = data[k % num][1] - data[(k - 1) % num][1]
        x2 = data[(k + 1) % num][0] - data[k % num][0]
        y2 = data[(k + 1) % num][1] - data[k % num][1]

        d_A = (x1 ** 2 + y1 ** 2) ** 0.5
        d_B = (x2 ** 2 + y2 ** 2) ** 0.5

        Vec_Cross = (x1 * y2) - (x2 * y1)

        sin_theta = Vec_Cross / (d_A * d_B)

        dv = sec_dis / sin_theta

        v1_x = (dv / d_A) * x1
        v1_y = (dv / d_A) * y1

        v2_x = (dv / d_B) * x2
        v2_y = (dv / d_B) * y2

        PQ_x = v1_x - v2_x
        PQ_y = v1_y - v2_y

        Q_x = data[k % num][0] + PQ_x
        Q_y = data[k % num][1] + PQ_y
        scal_data.append([Q_x, Q_y])
    return scal_data


if __name__ == '__main__':
    pass
